#!/usr/bin/env python

import math
import rospy
from geometry_msgs.msg import Twist

def subscriber_cb(data):
    """
    This is the callback of /cmd_vel subscriber
    param: data: Twist: linear speed and angular(radians)
    return: None
    """
    vel = Twist()
    vel.linear.x = data.linear.x * 600 + 1500
    vel.angular.z = math.degrees(data.angular.z) + 90
    pub.publish(vel)


def main():
    """
    Main function starts
    """
    global pub

    rospy.init_node('my_navigation_cmd')

    # Subscriber
    rospy.Subscriber('/cmd_vel', Twist, subscriber_cb, queue_size=1)
    
    # Publisher
    pub = rospy.Publisher('/car/cmd_vel', Twist, queue_size=1)
    """
    Main function ends
    """


if __name__ == '__main__':
    main()

    rospy.spin()
